﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
using System.Drawing;
using FLMScan.Geometry;
using FLMScan.OpenCV;
using log4net;
using FLMScan.Parameters;

namespace FLMScan.WorldCoordinateReconstruction.Riedmaier
{
    class CameraCalibrator
    {
        private ILog logger;
        private int calibrationImageCount;
        private int innerChessboardCornersX;
        private int innerChessboardCornersY;
        private int chessboardCornerCount;
        private double chessboardWorldSquareSize;
        private int imageWidth;
        private int imageHeight;        
        private Dictionary<int, List<FlmPoint>> chessboardCorners;        
        private IntrinsicCameraParameters intrinsicParameters;
        
        public CameraCalibrator(int innerCornersX, int innerCornersY, double chessboardSquareSize)
        {
            logger = LogManager.GetLogger(typeof(CameraCalibrator));

            innerChessboardCornersX = innerCornersX;
            innerChessboardCornersY = innerCornersY;
            chessboardCornerCount = innerChessboardCornersX * innerChessboardCornersY;
            chessboardWorldSquareSize = chessboardSquareSize;

            logger.Debug("Initializing CameraCalibrator");
            logger.DebugFormat("Number of chessboard corners x:{0}, y:{1}, total:{2}", innerChessboardCornersX, innerChessboardCornersY, chessboardCornerCount);
            logger.DebugFormat("Edge length of chessboard square: {0} mm", chessboardWorldSquareSize);

            chessboardCorners = new Dictionary<int, List<FlmPoint>>();
            intrinsicParameters = new IntrinsicCameraParameters();
        }

        public void Calibrate(String imageDirectoryPath)
        {
            string[] files = Directory.GetFiles(imageDirectoryPath);
            calibrationImageCount = files.Length;

            FlmMatrix chessBoardImagePoints = new FlmMatrix(1, calibrationImageCount * chessboardCornerCount);
            FlmMatrix chessBoardWorldPoints = new FlmMatrix(1, calibrationImageCount * chessboardCornerCount);
            
            logger.DebugFormat("Found {0} chessboard image files in {1}", calibrationImageCount, imageDirectoryPath);
            
            for (int i = 0; i < calibrationImageCount; i++)
            {
                logger.DebugFormat("Analyzing chessboard image file {0}", files[i]);

                Bitmap chessboardBitmap = (Bitmap)Bitmap.FromFile(files[i]);

                if (i == 0)
                {
                    imageWidth = chessboardBitmap.Width;
                    imageHeight = chessboardBitmap.Height;
                    logger.DebugFormat("Chessboard image file size (w x h): {0} x {1}", imageWidth, imageHeight);
                }
                else
                {
                    if ((chessboardBitmap.Width != imageWidth) || (chessboardBitmap.Height != imageHeight))
                    {
                        // TBD: handle this case
                        System.Diagnostics.Debug.Assert(false, "Mismatch of chessboard image file size.");
                        logger.ErrorFormat("Mismatch of chessboard image file size (w x h): {0} x {1}", chessboardBitmap.Width, chessboardBitmap.Height);
                    }                    
                }

                List<FlmPoint> currentCorners = CalibrationWrapperOpenCV.RetrieveChessboardCorners(chessboardBitmap, innerChessboardCornersX, innerChessboardCornersY);

                logger.DebugFormat("Detected {0} chessboard corners", currentCorners.Count);
                chessboardCorners.Add(i, currentCorners);                
            }            

            intrinsicParameters = CalibrationWrapperOpenCV.CalibrateCamera(calibrationImageCount, innerChessboardCornersX, innerChessboardCornersY, chessboardCorners, chessboardWorldSquareSize, imageWidth, imageHeight);

            logger.DebugFormat("Calculated intrinsic camera parameter focal length x:{0} y:{1}", intrinsicParameters.FocalLengthX, intrinsicParameters.FocalLengthY);
            logger.DebugFormat("Calculated intrinsic camera parameter primary point x:{0} y:{1}", intrinsicParameters.PrimaryPointX, intrinsicParameters.PrimaryPointY);

            int coefficientCount = 0;
            foreach (double distortionCoefficient in intrinsicParameters.DistortionCoefficients)
            {
                logger.DebugFormat("Calculated intrinsic camera parameter distortion {0}:{1}", coefficientCount++, distortionCoefficient);
            }
        }

        public IntrinsicCameraParameters IntrinsicParameters
        {
            get { return intrinsicParameters; }
        }                            
    }
}
